#include "util.h"
#include "string.h"
#include "sysconf.h"

uint16_t modbus_crc16(uint8_t* data, uint8_t length) {
  uint16_t crc = 0xFFFF;
  for(uint8_t i = 0; i < length; i++) {
    crc ^= data[i];
    for(uint8_t j = 0; j < 8; j++) {
      if(crc & 0x0001) {
        crc = (crc >> 1) ^ 0xA001;
      } else {
        crc >>= 1;
      }
    }
  }
  return crc;
}

void float_logic_to_modbus(float input, uint16_t* modbus_array, int index) {
  union {
    float    f;
    uint32_t u;
  } converter;
  converter.f = input;
  // 小端序 uint32_t 拆为 2 个 uint16_t，大端序排列
  modbus_array[index]     = (converter.u >> 16) & 0xFFFF; // 高 16 位
  modbus_array[index + 1] = converter.u & 0xFFFF;         // 低 16 位
}
void float_modbus_to_logic(uint16_t* modbus_array, float* output, int index) {
  union {
    float    f;
    uint32_t u;
  } converter;
  // 大端序 uint32_t 组合为 float
  converter.u = ((uint32_t) modbus_array[index] << 16) | (uint32_t) modbus_array[index + 1];
  *output     = converter.f;
}
// double 转 Modbus 大端序 uint16_t 寄存器
void double_logic_to_modbus(double input, uint16_t* modbus_array, int index) {
  union {
    double   d;
    uint64_t u;
  } converter;
  converter.d = input;

  // 小端序 uint64_t 拆为 4 个 uint16_t，大端序排列
  modbus_array[index]     = (converter.u >> 48) & 0xFFFF; // 最高 16 位
  modbus_array[index + 1] = (converter.u >> 32) & 0xFFFF;
  modbus_array[index + 2] = (converter.u >> 16) & 0xFFFF;
  modbus_array[index + 3] = converter.u & 0xFFFF; // 最低 16 位
}

// Modbus 大端序 uint16_t 寄存器转 double
void double_modbus_to_logic(uint16_t* modbus_array, double* output, int index) {
#pragma pack(push, 8)
  union {
    double   d;
    uint64_t u;
  } converter;
#pragma pack(pop)
  // 大端序 uint16_t 组合为 uint64_t（小端序存储）
  converter.u = ((uint64_t) modbus_array[index] << 48) | ((uint64_t) modbus_array[index + 1] << 32) |
                ((uint64_t) modbus_array[index + 2] << 16) | (uint64_t) modbus_array[index + 3];
  double temp __attribute__((aligned(8)));
  temp = converter.d;
  memcpy(output, &temp, sizeof(double)); // 安全复制
}
// uint32_t 转 Modbus 大端序 uint16_t 寄存器
void uint32_logic_to_modbus(uint32_t input, uint16_t* modbus_array, int index) {
  modbus_array[index]     = (input >> 16) & 0xFFFF; // 高 16 位
  modbus_array[index + 1] = input & 0xFFFF;         // 低 16 位
}

// Modbus 大端序 uint16_t 寄存器转 uint32_t
void uint32_modbus_to_logic(const uint16_t* modbus_array, uint32_t* output, int index) {
  *output = ((uint32_t) modbus_array[index] << 16) | modbus_array[index + 1];
}

// 小端序 int32_t 转 Modbus 大端序 uint16_t 寄存器
void int32_logic_to_modbus(int32_t input, uint16_t* modbus_array,
                           int index) {             // 转换逻辑与 uint32_t 相同
  modbus_array[index]     = (input >> 16) & 0xFFFF; // 高 16 位
  modbus_array[index + 1] = input & 0xFFFF;         // 低 16 位
}

// Modbus 大端序 uint16_t 寄存器 到小端序 int32_t
void int32_modbus_to_logic(const uint16_t* modbus_array, int32_t* output, int index) {
  *output = ((int32_t) modbus_array[index] << 16) | modbus_array[index + 1];
}

// 小端序 uint64_t 转 Modbus 大端序 uint16_t 寄存器（4 个寄存器）
void uint64_logic_to_modbus(uint64_t input, uint16_t* modbus_array, int index) {
  modbus_array[index]     = (input >> 48) & 0xFFFF; // 最高 16 位
  modbus_array[index + 1] = (input >> 32) & 0xFFFF;
  modbus_array[index + 2] = (input >> 16) & 0xFFFF;
  modbus_array[index + 3] = input & 0xFFFF;
}

// Modbus 大端序 uint16_t 寄存器（4 个寄存器）到小端序 uint64_t
void uint64_modbus_to_logic(const uint16_t* modbus_array, uint64_t* output, int index) {
  *output = ((uint64_t) modbus_array[index] << 48) | ((uint64_t) modbus_array[index + 1] << 32) |
            ((uint64_t) modbus_array[index + 2] << 16) | modbus_array[index + 3];
}

// 小端序 int64_t 转 Modbus 大端序 uint16_t 寄存器（4 个寄存器）
void int64_logic_to_modbus(int64_t input, uint16_t* modbus_array, int index) {
  modbus_array[index]     = (input >> 48) & 0xFFFF; // 最高 16 位
  modbus_array[index + 1] = (input >> 32) & 0xFFFF;
  modbus_array[index + 2] = (input >> 16) & 0xFFFF;
  modbus_array[index + 3] = input & 0xFFFF;
}

//  Modbus 大端序 uint16_t 寄存器（4 个寄存器）到小端序 int64_t
void int64_modbus_to_logic(const uint16_t* modbus_array, int64_t* output, int index) {
  *output = ((int64_t) modbus_array[index] << 48) | ((int64_t) modbus_array[index + 1] << 32) |
            ((int64_t) modbus_array[index + 2] << 16) | modbus_array[index + 3];
}

// 大端序（modbus_array）到小端序（logic_array），4 字节
void modbus_to_logic_4B(const uint16_t* modbus_array, uint16_t* logic_array, int index) {
  logic_array[index]     = modbus_array[index + 1]; // 低 16 位（B2B3）
  logic_array[index + 1] = modbus_array[index];     // 高 16 位（B0B1）
}

// 小端序（logic_array）到大端序（modbus_array），4 字节
void logic_to_modbus_4B(const uint16_t* logic_array, uint16_t* modbus_array, int index) {
  modbus_array[index]     = logic_array[index + 1]; // 高 16 位（B0B1）
  modbus_array[index + 1] = logic_array[index];     // 低 16 位（B2B3）
}

// 大端序（modbus_array）到小端序（logic_array），8 字节
void modbus_to_logic_8B(const uint16_t* modbus_array, uint16_t* logic_array, int index) {
  logic_array[index]     = modbus_array[index + 3]; // 最低 16 位（B6B7）
  logic_array[index + 1] = modbus_array[index + 2]; // 次低 16 位（B4B5）
  logic_array[index + 2] = modbus_array[index + 1]; // 次高 16 位（B2B3）
  logic_array[index + 3] = modbus_array[index];     // 最高 16 位（B0B1）
}

// 小端序（logic_array）到大端序（modbus_array），8 字节
void logic_to_modbus_8B(const uint16_t* logic_array, uint16_t* modbus_array, int index) {
  modbus_array[index]     = logic_array[index + 3]; // 最高 16 位（B0B1）
  modbus_array[index + 1] = logic_array[index + 2]; // 次高 16 位（B2B3）
  modbus_array[index + 2] = logic_array[index + 1]; // 次低 16 位（B4B5）
  modbus_array[index + 3] = logic_array[index];     // 最低 16 位（B6B7）
}
